#include <Servo.h>

/* Analog output pin */
int bPS;  // Bits per segment: (segment = values that control a specific axis)
int charUsage;  // Char Usage: number of numbers in a char that are used to send data
int numSPC;  // Number of Segments per Character: usually 4
int xCenter, xRange, xOffset;
int yCenter, yRange, yOffset;
int pin10 = 10; // movement x
int pin11 = 11; // movement y
int pin12 = 12; // camera x
int pin13 = 13; // camera y
int x;
int y;
int midp;
int dead;
int stopmove;
unsigned long t1;
unsigned long t2;
Servo s1, s2;

void setup()
{
  Serial.begin(9600);
  /* Make the pin an output */
  pinMode(pin10, OUTPUT);
  pinMode(pin11, OUTPUT);
  s1.attach(12);
  s2.attach(13);

  analogWrite(pin10, 160);
  analogWrite(pin11, 160);
  s1.write(90);
  s2.write(90);
  delay(1000);
  
  bPS = 40;
  midp = (bPS / 2) - 1;
  dead = bPS / 20;
  xCenter = 153;
  yCenter = 160;
  charUsage = 159;
  
  numSPC = bPS/charUsage;
  xRange = (255 - xCenter) * 2;
  yRange = (255 - yCenter) * 2;
  xOffset = 255 - xRange;
  yOffset = 255 - yRange;

  stopmove = 0;
}

void loop()
{
  byte in_value;
  byte write_value;
  
  if (Serial.available()) {
      t1 = millis();
      in_value = Serial.read();
      if (in_value < bPS) {
        
        write_value = (in_value * xRange / bPS) + xOffset;
        if ((in_value > (midp - dead)) && (in_value < (midp + dead))) { write_value = xCenter; }
        analogWrite(pin10, write_value);  // Write a movement x value
      }
      else if ((in_value >= bPS) && (in_value < 2*bPS)) {
        write_value = ((in_value - 1*bPS) * yRange / bPS) + yOffset;
        if ((in_value > (midp - dead)) && (in_value < (midp + dead))) { write_value = yCenter; }
        analogWrite(pin11, write_value);  // Write a movement y value
      }
      else if ((in_value >= 2*bPS) && (in_value < 3*bPS)) {
        write_value = ((in_value - 2*bPS) * 150 / bPS) + 20;
        s1.write(write_value); // Write a camera x value
      }
      else if ((in_value >= 3*bPS) && (in_value < 4*bPS)) {
        write_value = ((in_value - 3*bPS) * 150 / bPS) + 20;
        s2.write(write_value); // Write a camera y value
      }
  }else{
    if((t1 + 500) <= millis()){
      analogWrite(pin10, 160);
      analogWrite(pin11, 160);
    }
  }
}
